#include <ximera/physics/ode/entity.hpp>
#include <ximera/physics/ode/space.hpp>
#include <ximera/physics/ode/shape.hpp>

namespace ximera { namespace physics { namespace ode
{
    SharedPtr<ShapeFactory> Entity::_shapeFactory;

    Entity::Entity(Space* space, EntityType type)
        : _space(0),
        _type(type)
    {
        XIMERA_ASSERT_DEF(space);

		if (!_shapeFactory)
		{
			_shapeFactory.reset(new ShapeFactory());
            _shapeFactory->subscribe(ShapeType::Sphere, &ShapeFactory::defaultCreator<Sphere>);
            _shapeFactory->subscribe(ShapeType::Box, &ShapeFactory::defaultCreator<Box>);
            _shapeFactory->subscribe(ShapeType::Plane, &ShapeFactory::defaultCreator<Plane>);
            _shapeFactory->subscribe(ShapeType::Mesh, &ShapeFactory::defaultCreator<Mesh>);
		}

        attach(space);

        if (type == EntityType::Dynamic)
        {
            // create the dynamic body
            _body.reset(new RigidBody(_space->odeWorld()));
        }        
    }

    Entity::~Entity()
    {
        if (_space)
            _space->removeEntity(this);

        removeAllShapes();        
    }

    EntityType Entity::type() const
    {
        return _type;
    }

    ISpace* Entity::space() const
    {
        return _space;
    }

    IRigidBody* Entity::body() const
    {
        return _body.get();
    }

    UInt32 Entity::shapeCount() const
    {
        return (UInt32)_shapes.size();
    }

    IShape* Entity::shape(UInt32 index) const
    {
        return _shapes[index];
    }

    void Entity::addShape(IShape* shape)
    {        
        XIMERA_ASSERT(shape, XIMERA_THROW(InvalidParamException, L"Parameter 'shape' must not be null"));
        XIMERA_ASSERT(!_body || shape->type() != ShapeType::Plane, 
            XIMERA_THROW(InvalidParamException, L"Attempting to add a non-placeable shape in a dynamic entity!"));

        if (std::find(_shapes.begin(), _shapes.end(), shape) != _shapes.end())
            return;
        
        IOdeShape* odeShape = dynamic_cast<IOdeShape*>(shape);
        XIMERA_ASSERT(odeShape, XIMERA_THROW(InvalidParamException, L"Invalid shape!"));

        if (odeShape->entity())
            odeShape->entity()->removeShape(shape);

        odeShape->attach(this);

        _shapes.push_back(shape);
    }

    void Entity::removeShape(IShape* shape)
    {
        XIMERA_ASSERT(shape, XIMERA_THROW(InvalidParamException, L"Parameter 'shape' must not be null"));        

        IOdeShape* odeShape = dynamic_cast<IOdeShape*>(shape);
        XIMERA_ASSERT(odeShape, XIMERA_THROW(InvalidParamException, L"Invalid shape!"));

        if (odeShape->entity() != this)
            return;

        odeShape->detach();

        _shapes.erase(std::remove(_shapes.begin(), _shapes.end(), shape), _shapes.end());
    }

    void Entity::removeAllShapes()
    {
        for (ShapeContainer::iterator it = _shapes.begin(); it != _shapes.end(); ++it)
        {
            IShape* shape = *it;
            delete shape;
        }

        _shapes.clear();
    }

    IShape* Entity::createShape(ShapeType type)
    {
        XIMERA_ASSERT(!_body || type != ShapeType::Plane, 
            XIMERA_THROW(InvalidParamException, L"Attempting to add a non-placeable shape in a dynamic entity!"));

        IShape* shape = _shapeFactory->create(type);
        XIMERA_ASSERT_DEF(shape);

        IOdeShape* odeShape = dynamic_cast<IOdeShape*>(shape);
        odeShape->attach(this);

        _shapes.push_back(shape);

        return shape;
    }

    void Entity::attach(Space* space)
    {
        _space = space;

        for (ShapeContainer::iterator it = _shapes.begin(); it != _shapes.end(); ++it)
        {
            IOdeShape* shape = dynamic_cast<IOdeShape*>(*it);
            shape->attachSpace();
        }
    }

    void Entity::detach()
    {
        for (ShapeContainer::iterator it = _shapes.begin(); it != _shapes.end(); ++it)
        {
            IOdeShape* shape = dynamic_cast<IOdeShape*>(*it);
            shape->detachSpace();
        }

        _space = 0;
    }
}}}
